/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: SpeedCtrlClientExample.cpp,v 1.4 2009-07-22 15:37:35 morsko Exp $
 *
 * \file
 * \brief This is an example for using the SpeedCtrl interface.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "SpeedCtrlClient.hpp"
#include <curses.h>
#include <math.h>
//*****************************************************************************
#define GIMI_CLIENT_API 20000
#include "gimi.h"
//*****************************************************************************
// Objects

static gimi::GIMI g;
static MaCI::SpeedCtrl::CSpeedCtrlClient scc(&g, 0);

// Speed limit value +/-
static float v_lim = 0.5;

// Angular speed limit value +/-
static float w_lim = 3.14159;

// Current speed
static float v = 0.0;

// Current angular speed
static float w = 0.0;

// Current acceleration
static float a = 0.0;
static float a_lim = 0.2;

//Speed event sequence 
static int speedSeq = -1;
//*****************************************************************************
//*****************************************************************************
static const char *strOK = "OK.\n";
static const char *strFail = "FAILED!\n";
//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
static volatile bool run = true;
static ownThreadHandle  speedSendingThread;
static bool setSpeedCommanded = false;
//*****************************************************************************

static std::string macigroup = "";
static std::string groupinstance = "";

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////

static int ocnt=0;
static int oax=0;
static int oay=0;
static int oaz=0;

static const char *accel_filename = "/sys/class/i2c-adapter/i2c-3/3-001d/coord";

int liqaccel_read(int *ax, int *ay,int *az) {
  FILE *fd;
  int rs;
  fd = fopen(accel_filename, "r");
  if(fd==NULL){ fprintf(stderr, "liqaccel, cannot open for reading"); return -1;} 
  rs=fscanf((FILE*) fd,"%i %i %i",ax,ay,az);      
  fclose(fd);     
  if(rs != 3){ fprintf(stderr, "liqaccel, cannot read information"); return -2;}
  int bx=*ax;
  int by=*ay;
  int bz=*az;
  if(ocnt>0)
    {
      /*
       *ax=oax+(bx-oax)*0.1;
       *ay=oay+(by-oay)*0.1;
       *az=oaz+(bz-oaz)*0.1;
       */
    }
  oax=*ax;
  oay=*ay;
  oaz=*az;
  ocnt++;

  return 0;
}

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////



static void ExecuteSetSpeed()
{
  // Cut the speed to limits
  if (v > v_lim) v = v_lim;
  else if (v < -v_lim) v = -v_lim;
  if (w > w_lim) w = w_lim;
  else if (w < -w_lim) w = -w_lim;

  // Print speeds
  printw("\nSetting Speed to v:%.2f, W:%.4f\n", 
         v, w);
  
  // Print stopped if stopped
  if (v == 0.00 && w == 0.0) printw(" (STOPPED)");
  
  // LF
  /*printw(" ... ");
  
  // Command!
    if (scc.SetSpeed(v, w, a) == true) {
    printw(strOK);
  } else {
    printw(strFail);
    }*/
}

//*****************************************************************************

static void ExecuteSetAcceleration()
{
  // Cut to limit
  if (a > a_lim) a = a_lim;
  else if (a < 0.0) a = 0.0;
  
  printw("Setting Acceleration to a:%.4f ... ", 
         a);
 
  // No real command here, re-execute the setspeed now.
  //ExecuteSetSpeed();
}
//*****************************************************************************

static void DoSetAcceleration() 
{
  printw("Enter acceleration (a): ");
  nodelay(stdscr, 0);
  int f = scanw("%f", &a);
  nodelay(stdscr, 1);

  if (f == 1) {
    ExecuteSetAcceleration();

  } else {
    printw("Invalid input!\n");

  }
}
//*****************************************************************************

static void DoSetSpeed()
{
  printw("Enter speed (v w): ");
  nodelay(stdscr, 0);
  int f = scanw("%f %f", &v, &w);
  nodelay(stdscr, 1);

  if (f == 2) {
    ExecuteSetSpeed();
    setSpeedCommanded = true;
  } else {
    printw("Invalid input!\n");

  }
}
//*****************************************************************************

static void DoSetPower(const int value) 
{ 
  bool powerstate = false;
  switch(value) {
  case 'O':
    powerstate = true;
    break;
  }

  printw("Setting Power '%s' ... ", 
         powerstate?"ON":"OFF");

  // \todo No Implementation for SetPower()
}
//*****************************************************************************

static void DoSetSpeedEvent(const int value)
{
  bool result;
  bool state;
  switch(value){
  case 'P':{
    result = scc.StartGettingSpeedEvent();
    state = true;
    break;
  }
  case 'p':{
    result = scc.StopGettingSpeedEvent();
    state = false;
    break;
    
  }
  }
  printw("%s getting speed event... %s\n", 
         state?"Start":"Stop",result?"OK":"FAILED");

}
//*****************************************************************************

static void DoGetSpeedEvent()
{
  unsigned int timeout = 1000;
  printw("Getting speeds (timeout %d ms)...",timeout);
  float speed, angularSpeed,acceleration;
  MaCI::Common::TTimestamp tstamp;
  if(scc.GetSpeed(speed,angularSpeed,acceleration,tstamp,
                   NULL,timeout)){
    printw("OK!\n");
    printw("Speed: %f, Angular speed: %f, Acceleration: %f, Timestamp s:%d, us:%d \n",speed,angularSpeed, acceleration,tstamp.timestamp_s,tstamp.timestamp_us);
  }else{
    printw("FAILED!\n");
  }

}
//*****************************************************************************
static void DoStatus()
{
  printw("Status:\n");
  printw("\tSpeed: v:%.2f, w:%.4f\n", v, w);
  printw("\tAcceleration: a:%.4f\n", a);
}
//*****************************************************************************

static void DoUnknown(const int value)
{
  switch(value) {
  case 'q':
    printw("Unknown command 'q', did you mean _CAPITAL_ 'Q' as Quit?\n");
    break;

  default:
    if (isprint(value)) {
      printw("Unknown command '%c'\n", value);
    } else {
      printw("Unknown command '%x'\n", value);
    }
    break;
  }

}
//*****************************************************************************

static void DoSpeedCtrl(const int value)
{
  // Switch
  switch(value) {
  case KEY_UP:
    v += 0.01 + (0.05*fabsf(v));
  setSpeedCommanded = true;
    break;

  case KEY_DOWN:
    v -= 0.01 + (0.05*fabsf(v));
  setSpeedCommanded = true;
    break;

  case KEY_LEFT:
    w += 0.015 + (0.1*fabsf(w));
  setSpeedCommanded = true;
    break;

  case KEY_RIGHT:
    w -= 0.015 + (0.1*fabsf(w));
  setSpeedCommanded = true;
    break;
    
  case ' ':
    printw("Setting STOP ...");
    if (scc.SetStop()) {
      setSpeedCommanded = false;
      printw(strOK);
    } else {
      printw(strFail);
    }
    // Intentionally no break here.
    
  case 'z':
    setSpeedCommanded = false;
    w = 0.00;
    v = 0.00;
    break;
  }

  // Now, call the executor
   ExecuteSetSpeed();
}

static void DoSpeedCtrlN900(float speed, float angle)
{

  // Switch
  v = speed;
  w = angle;
  setSpeedCommanded = true;

  // Now, call the executor
  ExecuteSetSpeed();
}
//*****************************************************************************

static void DoAccelerationCtrl(const int value)
{
  // Switch.
  switch(value) {
  case KEY_NPAGE:
    a -= 0.01 + (0.005 * a);
    break;
    
  case KEY_PPAGE:
    a += 0.01 + (0.005 * a);
    break;
  }
  
  // Call the executor
  ExecuteSetAcceleration();
}
//*****************************************************************************

static void DoGetTimeoutTime()
{
  unsigned int timeout = 1000;
  //unsigned int reply;
  ownTime_ms_t reply;
  printw("Getting timeouttime (timeout %d ms)...",timeout);

  if(scc.GetSpeedCtrlServerTimeoutTime(reply,timeout)){
    printw("OK!\n");
    printw("Server-side timeout time: %u\n",reply);
  }else{
    printw("FAILED!\n");
  }
}
//*****************************************************************************
static void DoResendAuthentication()
{
  MaCI::MaCICtrl::TAccess newacc;
  scc.ValidateClientInterfaceAuthentication(newacc);
  if (newacc.IsAccess()) {
    printw("Succesfully Validated Authentication.\n");
  } else {
    printw("Failed to Validate Authentication - Service won't Accept your commands!\n");
  }
}
//*****************************************************************************

static void DoQuit()
{
  printw("\nQUIT.\n");
}
//*****************************************************************************

static void DoHelp()
{
  printw("\n******** Help - ");
  
  printw("Main ********\n");
  printw("Driving:\n");
  printw("'UP'\t- Accelerate\n");
  printw("'DOWN'\t- Decelerate\n");
  printw("'LEFT'\t- Turn left\n");
  printw("'RIGHT'\t- Turn right\n");
  printw("'PGUP'\t- Increase acceleration\n");
  printw("'PGDN'\t- Decrease acceleration\n");
  printw("'SPACE'\t- STOP & Zero speeds (== Fast Stop)\n");
  printw("'z'\t- Zero speeds (== Stop)\n");
  printw("\nSettings:\n");
  printw("'O'\t- Set PowerOn\n");
  printw("'o'\t- Set PowerOff\n");
  printw("'a'\t- Set Acceleration\n");
  printw("'s'\t- Set Speed\n");
  printw("'P'\t- Start to get speed event\n");
  printw("'p'\t- Stop getting speed event\n");
  printw("\nGettings:\n");
  printw("'g'\t- Get Speed Event\n");
  printw("'S'\t- Show Status (Speed, Acceleration)\n");
  printw("'t'\t- Get server-side timeout time\n");
  printw("\nManaging:\n");
  printw("'v'\t- Resend Authentication\n");

  
  printw("\nGeneral:\n");
  printw("'Q'\t- Quit\n");
  printw("'H'\t- This help\n");
  printw("\n");
}
//*****************************************************************************

/**
 * Returns if the proximity sensor state is Open or
 * closed. 
 *
 * @return true if status is closed, otherwise return false.
 */
bool proximitySensorClosed() {
  //FILE *fd;
  char proximity_state;
  FILE *fd = fopen("/sys/devices/platform/gpio-switch/proximity/state", "r");
  if(fd!=NULL) {
    fscanf(fd, "%c", &proximity_state);
    fclose(fd);
  }else {
    return false;
  }

  if(proximity_state == 'c') {
    return true;
  }
  return false;
}

static void DoSpeedCtrl()
{
  // Initialize Curses (After this point, don't use printf family calls!)
  initscr(); 
  cbreak(); 
  //noecho();
  nonl();
  intrflush(stdscr, FALSE);
  keypad(stdscr, TRUE);
  scrollok(stdscr, TRUE);
  //nodelay(stdscr, TRUE);
  //halfdelay(10);

  // After this point, we don't want dPrint to print to Screen, so we disable it.
    debugSetPrintLevel(0);
  
  clear();
  printw("Welcome to SpeedCtrl test program.\n\n");
  printw("For a quick 'help', give 'H' at any time. type 'Q' to Quit\n\n");

  bool proximitySensorClosedFirstTime = true;
  int axi = 0, ayi = 0, azi =0;
  do {
    int value;
    
    if(proximitySensorClosed()) {
      printw("Proximity sensor pressed.\n");
      float speed = 0;
      float angle = 0;
      int ax = 0, ay = 0, az =0;
      if(proximitySensorClosedFirstTime) {
	liqaccel_read(&axi, &ayi, &azi);
	proximitySensorClosedFirstTime = false;
      }else {
	liqaccel_read(&ax, &ay, &az);

	speed = (float)(ax-axi)/1000.0;
	if(fabs(speed) < 0.1) {
	  speed = 0;
	}else if(speed > 0.1) {
	  speed -= 0.1;
	}else if(speed < -0.1) {
	  speed += 0.1;
	}

	angle = (float)(ay-ayi)/1000.0;
	DoSpeedCtrlN900(speed, angle);
      
      }
      ownSleep_ms(250);    
      refresh();
      continue;
    }else {
      printw("Proximity sensor NOT pressed.\n");
      proximitySensorClosedFirstTime = true;
    }

    // Request new command.
    printw("\n\rInput: ");
    value = getch();

    if (value != ERR) {

      // We got one character; so now flush the input.
      int flush = 0;
      nodelay(stdscr,TRUE);
      while(getch() != ERR) flush++;
      nodelay(stdscr,FALSE);
      if (flush) {
        printw("(Ignored %d bytes of keyboard input)\n", flush);
      }

      // Terminate command line
      printw("\n");

      switch(value) {
      case ' ':
      case KEY_UP:
      case KEY_DOWN:
      case KEY_LEFT:
      case KEY_RIGHT:
      case 'z':
        DoSpeedCtrl(value);
        break;

      case KEY_NPAGE:
      case KEY_PPAGE:
        DoAccelerationCtrl(value);
        break;
      
      case 'O':
      case 'o':
        DoSetPower(value);
        break;

      case 'a':
        DoSetAcceleration();
        break;
        
      case 's':
        DoSetSpeed();
        break;

      case 'P':
      case 'p':{
        DoSetSpeedEvent(value);
        break;
      }
      case 'g':{
        DoGetSpeedEvent();
        break;
      }
      case 'S':
        DoStatus();
        break;

      case 'v':
        DoResendAuthentication();
        break;

      case 't':
        DoGetTimeoutTime();
        break;

      case 'H':
      case 'h':
        DoHelp();
        break;
   
      case 'Q':
        DoQuit();
        run = false;
        break;


      default:
        DoUnknown(value);
        break;
      }
    }
  } while(run);

  // Close curses.
  endwin();

}
//*****************************************************************************
static int SpeedSendingThread(void *)
{
  unsigned int sleepTime = 50;
  //  unsigned int timeoutTime;
  ownTime_ms_t timeoutTime;
  if(!scc.GetSpeedCtrlServerTimeoutTime(timeoutTime,1000)){
    return 0;
  }

  if(timeoutTime < sleepTime){
    sleepTime = timeoutTime/2;
  }

  while(run){
    if(setSpeedCommanded){
      // Cut the speed to limits
      if (v > v_lim) v = v_lim;
      else if (v < -v_lim) v = -v_lim;
      if (w > w_lim) w = w_lim;
      else if (w < -w_lim) w = -w_lim;
      
      scc.SetSpeed(v,w,a);
    }
    ownSleep_ms(sleepTime);
  }
  return 0;

}
//*****************************************************************************
void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tSpeedCtrlClientExample [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-s name]\t\t\tUse 'name' as SpeedCtrl target\n");
  fprintf(stderr,"  [-K key]\t\t\tUse 'key' as access key for SpeedCtrl interface\n");
  fprintf(stderr,"  [-G name]\t\t\tSpecify the MaCIGroup for SpeedCtrl target\n");
  fprintf(stderr,"  [-I name]\t\t\tSpecify the MaCIGroup Interface Instance\n");
  

  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  using namespace MaCI;
  // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 40002;
  std::string gimnetName = "";
  std::string gimnetSourceName = "";
  
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(8);
  debugSetLogFilename("SpeedCtrlClientExample.log");

  // Key.
  unsigned char akey[16];
  memset(akey,0x00,sizeof(akey));
  
  // Greet
  fprintf(stderr,
	  "\nSpeedCtrlClientExample - MaCI SpeedCtrl Interface Example - v0.0\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "s:n:u:p:G:I:K:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

   case 'K':
      strncpy((char*)akey, optarg, sizeof(akey));
      dPrint(3,"Using MaCI Key: '%s'", optarg);
      break;

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 's':
      gimnetSourceName = optarg;
      dPrint(3,"Using GIMnet source name '%s'", optarg);
      break;

    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

    case 'G':
      macigroup = optarg;
      dPrint(3,"Using MaCI-Group '%s'", optarg);
      break;

    case 'I':
      groupinstance = optarg;
      dPrint(3,"Using MaCI-Group Interface Instance '%s'", optarg);
      break;


      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  
  MaCI::EMaCIError e;
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());


    // Set target
    if (macigroup.size() && groupinstance.size()){
  
      std::string maciname;
      //sprintf(maciname, "%s.MaCI_SpeedCtrl.%s", macigroup, groupinstance);
      maciname = macigroup + ".MaCI_SpeedCtrl." + groupinstance;
      scc.SetDefaultTarget(MaCI::MaCICtrl::SMaCISL(maciname), 14000);
    }
    else{ 
  
      scc.SetDefaultTarget(gimnetSourceName);
    }
    //   scc.SetClientInterfaceAuthentication(MaCI::MaCICtrl::TAuthentication(akey, sizeof(akey)));

    // Attempt to Open Client
    if ( (e = scc.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open SpeedCtrlClient instance! (%s)",
	     GetErrorStr(e).c_str());
      
    } else {

      //THIS IS FOR TESTING
      MaCI::SpeedCtrl::TSpeed max;
      MaCI::SpeedCtrl::TSpeed min;

      if(scc.GetSpeedLimits(max,min)){
        v_lim = max.speed;
        w_lim = max.angularSpeed;
        a_lim = max.acceleration;
        dPrint(3,"SpeedLimits max %f %f %f, min %f %f %f", max.speed,max.angularSpeed,max.acceleration,
               min.speed,min.angularSpeed,min.acceleration);
      }else{
        dPrint(3,"Get speed limits returned false...");
      }
#ifdef OLD_IMPLEMENTATION
      const float v_lim = 0.1, w_lim = 1.0, a_lim = 0.1;
      float v=0.0, w=0.0, a=0.1;
      float dv=0.01, dw=0.0, da=0.0;
      int interval = 1;
      while(run) {
        // Foo.
        v += dv;
        w += dw;
        a += da;

        // Limits
        if (v >= v_lim) { 
          // Change dir
          dv = -dv;

        } else if (v <= -v_lim) {
          // Stop and start turning
          v = 0.0;
          dv = 0.0;

          dw = 0.1;
        } else if (w >= w_lim) { 
          dw = -dw;

        } else if (w <= -w_lim) {
          w = 0.0;
          dw = 0.0;

          dv = 0.01;
        }

        // Forward.
        dPrint(1,"Calling: SetSpeed(%.2f, %.2f, %.3f)",v,w,a);
        scc.SetSpeed(v,w,a);
  

        // Foo
        ownSleep_ms(interval);
      }
      
#else
      // Start signal handler thread
      speedSendingThread = ownThread_Create((void*)SpeedSendingThread, NULL);
      DoSpeedCtrl();
#endif
      
    }
  }
  
  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

